#include <algorithm>
#include <iostream>
#include<opencv2/opencv.hpp>
#include <iomanip>
#include <stdint.h>
#include <vector>

using namespace std;
using namespace cv;

class Square
{
public:
    Square(Rect r, Point c, Vec3b cv, int _area) :
        rect(r), center(c), center_value(cv), area(_area)
    {

    }

    Rect rect;
    Point center;
    Vec3b center_value;
    int area;
};

static Mat buf_to_mat(long *buf, int width, int height);
static bool find_special(Mat &img, long &x, long &y);
static int find_special(int number, Mat &stats, Mat &centroids, Mat &img, vector<Square> &square_list);

bool core_detect(long *buf, int width, int height, long &x, long &y)
{
    int64 begTick, endTick;

    begTick = getTickCount();

	Mat img = buf_to_mat(buf, width, height);
    bool ret = false;

	//imwrite("capture.jpg", img);
    ret = find_special(img, x, y);

    endTick = getTickCount();
    //cout << "spend:" << (endTick - begTick) / getTickFrequency() * 1000 << "ms" << endl;

    return ret;
}

static Mat buf_to_mat(long *buf, int width, int height)
{
	Mat img(height, width, CV_8UC3);

	unsigned long *p_buf = (unsigned long *)buf;
	for (int y = 0; y < height; y++)
	{
		Vec3b *p_img = img.ptr<Vec3b>(y);
		for (int x = 0; x < width; x++)
		{
			(*p_img)[2] = (*p_buf >> 16) & 0xff;
			(*p_img)[1] = (*p_buf >> 8) & 0xff;
			(*p_img)[0] = (*p_buf >> 0) & 0xff;

			p_buf++;
			p_img++;
		}
	}

	return img;
}

static bool find_special(Mat &img, long &x, long &y)
{
    cv::Mat /*img_gray,*/ img_edge, labels, centroids, img_color, stats;
    bool ret = false;
    int64 beg_tick, end_tick;

    /*beg_tick = getTickCount();
    cv::cvtColor(img, img_gray, cv::COLOR_BGR2GRAY);
    end_tick = getTickCount();
    cout << "BGR2GRAY spend: " << (end_tick - beg_tick) / getTickFrequency() * 1000 << "ms" << endl;*/

    //cv::threshold(img, img_edge, 128, 255, cv::THRESH_BINARY);
    //img_edge = img_gray != 221;

    beg_tick = getTickCount();
    img_edge.create(Size(img.cols, img.rows), CV_8UC1);
    for (int i = 0; i < img.rows; i++)
    {
        Vec3b *ptr_rgb = img.ptr<Vec3b>(i);
        unsigned char *ptr_edge = img_edge.ptr<unsigned char>(i);
        for (int j = 0; j < img.cols; j++)
        {
            *ptr_edge =
                *ptr_rgb != Vec3b(221, 221, 221)
                && *ptr_rgb != Vec3b(96, 96, 240)
                ? 255 : 0;

            ptr_rgb++;
            ptr_edge++;
        }
    }
    end_tick = getTickCount();
    //cout << "create img_edge spend: " << (end_tick - beg_tick) / getTickFrequency() * 1000 << "ms" << endl;

    //cv::Canny(img_edge, img_edge, 10, 20);
    //cv::imshow("Image after threshold", img_edge);

    beg_tick = getTickCount();
    int nccomps = cv::connectedComponentsWithStats(
        img_edge,
        labels,
        stats,
        centroids
    );
    end_tick = getTickCount();
    //cout << "Total Connected Components Detected: " << nccomps << ", spend: " << (end_tick - beg_tick) / getTickFrequency() * 1000 << "ms" << endl;

    beg_tick = getTickCount();
    vector<Square> square_list;
    int special_index = find_special(nccomps, stats, centroids, img, square_list);
    end_tick = getTickCount();
    //cout << "special index=" << special_index << ", spend: " << (end_tick - beg_tick) / getTickFrequency() * 1000 << "ms" << endl;

    if (special_index != -1)
    {
        rectangle(img, square_list[special_index].rect, Scalar(0, 0, 255), 5);
        x = square_list[special_index].center.x;
        y = square_list[special_index].center.y;
        ret = true;
    }

    //vector<cv::Vec3b> colors(nccomps + 1);
    //colors[0] = cv::Vec3b(0, 0, 0); // background pixels remain black.
    //for (int i = 1; i < nccomps; i++) {
    //	colors[i] = cv::Vec3b(rand() % 256, rand() % 256, rand() % 256);
    //	int area = stats.at<int>(i, cv::CC_STAT_AREA);
    //	//if (area < 100 || area > 1500)
    //	//{
    //	//	colors[i] = cv::Vec3b(0, 0, 0); // small regions are painted with black too.
    //	//	continue;
    //	//}
    //	int cx = (int)centroids.at<double>(i, 0);
    //	int cy = (int)centroids.at<double>(i, 1);
    //	int cv = img_gray.at<unsigned char>(cv::Point(cx, cy));
    //	cout << "[" << setw(3) << i << "] " << "area=" << area << ", center value=" << cv << endl;
    //}
    //img_color = cv::Mat::zeros(img.size(), CV_8UC3);
    //for (int y = 0; y < img_color.rows; y++)
    //	for (int x = 0; x < img_color.cols; x++)
    //	{
    //		int label = labels.at<int>(y, x);
    //		CV_Assert(0 <= label && label <= nccomps);
    //		img_color.at<cv::Vec3b>(y, x) = colors[label];
    //	}

    //cv::imshow("Labeled map", img_color);
    return ret;
}

static int find_special(int number, Mat &stats, Mat &centroids, Mat &img, vector<Square> &square_list)
{
    Vec3b center_value(0, 0, 0);
    int repeat_num = 0;

    for (int i = 1; i < number; i++)
    {
        int *stat = stats.ptr<int>(i);
        int area = stat[CC_STAT_AREA];
        if (area < 200)
        {
            continue;
        }

        Point center((int)centroids.at<double>(i, 0), (int)centroids.at<double>(i, 1));
        Rect rect(stat[CC_STAT_LEFT], stat[CC_STAT_TOP], stat[CC_STAT_WIDTH], stat[CC_STAT_HEIGHT]);
        Vec3b cv = img.at<Vec3b>(center);
        if (cv == center_value)
        {
            repeat_num++;
        }
        else
        {
            if (repeat_num < 2)
            {
                center_value = cv;
                repeat_num = 1;
            }
            else
            {
                //nothing 
            }
        }

        Square square(rect, center, cv, area);
        //cout << "[" << setw(3) << square_list.size() << "] " << "area=" << area << ", center value=" << cv << endl;
        square_list.push_back(square);
    }

    if (square_list.empty())
    {
        cerr << "Not find any square at all!!!!" << endl;
        return -1;
    }


    int special_index = -1;
    for (int i = 0; i < square_list.size(); i++)
    {
        if (square_list[i].center_value != center_value)
        {
            special_index = i;
            break;
        }
    }

    return special_index;
}